#!/usr/bin/python
# -*- mode:python; coding:utf-8 -*-

# NOTE: see https://code.ros.org/trac/opencv/ticket/373 if get
#  "cv.error: Non-positive width or height"

import cv
import sys
import math

def getSquare(p1, p3):
    x1, y1 = p1
    x3, y3 = p3

    r = math.sqrt((x3-x1)**2 + (y3-y1)**2) / 2
    xc, yc = x1 + (x3-x1)/2, y1 + (y3-y1)/2
    m = (x3-x1) / float(y1-y3)

    cosm = r * r * (y1-y3) / (r * math.sqrt((y1-y3)**2 + (x3-x1)**2))

    x = xc + cosm
    y = m*x - m*xc + yc
    p2 = int(x), int(y)

    x = xc - cosm
    y = m*x - m*xc + yc
    p4 = int(x), int(y)

    return p1, p2, p3, p4

c = cv.CreateCameraCapture(0)
if not c:
    print "Camera not found!"
    sys.exit(-1)

cv.NamedWindow("Test 08")
cv.StartWindowThread()

while True:
    f = cv.QueryFrame(c)
    if not f: break

    gf = cv.CreateImage((f.width, f.height), cv.IPL_DEPTH_8U, 1)
    cv.CvtColor(f, gf, cv.CV_RGB2GRAY)

    try:
        keys = cv.ExtractSURF(gf, None, cv.CreateMemStorage(), (0, 300, 3, 2))[0]
        if len(keys) == 2:
            p1 = tuple(map(int, keys[0][0]))
            p3 = tuple(map(int, keys[1][0]))
            p1, p2, p3, p4 = getSquare(p1, p3)
            cv.PolyLine(f, [[p1, p2, p3, p4]], 1, 125, 2)

    except cv.error:
        pass

    cv.ShowImage("Test 08", f)
    d = 1000 / 20
    k = cv.WaitKey(d)
    if k != -1: break
